/*
 * 本任务从Mti读取数据，当成功接收一组数据后返回给等待数据的任务。
 * 等待数据的任务可以有多个，通过接口函数 mti_add_noticer来定义。
 * 发送的消息不需要确认(Ack)，消息附带的数据是Mti的Z轴转角。
 */
#include "tasks.h"
#include "devices.h"
#include <devices_ext/mti.h>
#include <string.h>
#include <stdlib.h>

static const uint8_t mport = 1;
/* 定义MTI消息空间 */
DEF_MTI_MSG( recv, 44 );
static Mutex _notice_lock = SEM_DONE;
static MtiNoticer *_notice = NULL;
int _yaw;
static void notice_all( void );

DEF_TASK( mti, 300, 0 )
{
	MTIM_INIT( recv );
	uart_open( mport, UModeRW );
#if WANT_CONFIG
	sleep_ms( &tmr, 1000 );
	mti_send( mport, send );
	if( mti_recv( mport, recv) == MtiMidGoToConfigAck ){
		uart_write( uport, "MtiConfigMode!\n", 15 );
		while( true ){
			int rid = mti_recv( uport, recv );
			if( rid >= 0 ){
				mti_send( mport, recv );
				if( rid == MtiMidGoToMeasurement ){
					uart_write( uport, "MtiMeasureMode!\n", 16 );
					break;
				}
				mti_recv( mport, recv );
				mti_send( uport, recv );
			} else {
				uart_write( uport, "MtiError!\n", 10 );
			}
		}
	}
#endif

	while( true ){
		if( mti_recv( mport, recv ) == MtiMidMTData ){
			_yaw = mtim_get_int( recv, 2 );
			notice_all();
		}
	}
}

SysMsg mtimsg = {
		.id = MTI_MID,
		.data.val = 181,
};

/* 向所有等待Mti的任务发消息 */
static void notice_all( void )
{
	mtimsg.data.val = _yaw;
	wait_mutex( &_notice_lock );
	MtiNoticer *p = _notice;
	while( p != NULL ){
		send_sys_msg( p->target, &mtimsg );
		p = p->next;
	}
	set_mutex( &_notice_lock );
}

/* 添加要通知的目标，使用InsertBefore方法 */
void mti_add_noticer( MtiNoticer *notice )
{
	wait_mutex( &_notice_lock );
	notice->next = _notice;
	_notice = notice;
	set_mutex( &_notice_lock );
}

/* 删除掉一个noticer */
void mti_remove_noticer( MtiNoticer *notice )
{
	wait_mutex( &_notice_lock );
	MtiNoticer *p = _notice;
	while( p != NULL ){//遍历整个单链表
		if( p->next == notice ){//一个结点的后面的结点是目标结点，解除其链接
			p->next = notice->next;
			notice->next = NULL;
			break;
		}
		p = p->next;
	}
	set_mutex( &_notice_lock );
}

int get_yaw( void )
{
	return _yaw;
}
